#include <cmath>
#include <stdlib.h>
#include "structure.h"
#include "mcl.h"
#include "const.h"

#ifndef M_PI
#define M_PI 3.14159265358979
#endif

#define SQUARE(x) ((x)*(x))

int  int_random(int max)
{
  return (int)(max*(rand()/(RAND_MAX+1.0)));
}


double uniform_random(double min, double max)
{
  return min + (rand() / (double)RAND_MAX) * (max - min);
}


double gaussian_random(double mean, double std)
{
  const double norm = 1.0 / (RAND_MAX + 1.0);
  double u = 1.0 - rand() * norm;                  /* can't let u == 0 */
  double v = rand() * norm;
  double z = sqrt(-2.0 * log(u)) * cos(2.0 * M_PI * v);
  return mean + std * z;
} 


inline double normalize_theta_rad(double theta) 
{
  double multiplier;

  if (theta >= -M_PI && theta < M_PI)
    return theta;

  multiplier = floor(theta / (2*M_PI));
  theta = theta - multiplier*2*M_PI;
  if (theta >= M_PI)
    theta -= 2*M_PI;
  if (theta < -M_PI)
    theta += 2*M_PI;

  return theta;
} 


inline
double normalize_theta_deg(double theta)

{
  double multiplier;

  if (theta >= -180.0 && theta < 180.0)
    return theta;

  multiplier = floor(theta / (2*180.0));
  theta = theta - multiplier*2*180.0;
  if (theta >= 180.0)
    theta -= 2*180.0;
  if (theta < -180.0)
    theta += 2*180.0;

  return theta;
} 


//

LocalizerParticleFilter *
localizer_particle_filter_new(int num_particles)
{
  LocalizerParticleFilter *filter;

  filter = 
    (LocalizerParticleFilter *)
    calloc(1, sizeof(LocalizerParticleFilter));

  filter->num_particles = num_particles;
  filter->particles = (LocalizerParticle *)
    calloc(filter->num_particles, sizeof(LocalizerParticle));

  filter->initialized = 0;
  filter->ready = 0;

  return filter;
     
}

void
localizer_particle_filter_delete(LocalizerParticleFilter *filter)
{

  free(filter->particles);
  free(filter);

}


void localizer_initialize_particles_gaussians(LocalizerParticleFilter *filter,
					      int num_modes,
					      Point *mean,
					      Point *std)
{
  int i, j, each, start, end;
  float x, y, theta;

  each = (int)floor(filter->num_particles / (float)num_modes);
  for(i = 0; i < num_modes; i++) {
    start = i * each;
    if(i == num_modes - 1)
      end = filter->num_particles;
    else
      end = (i + 1) * each;

    for(j = start; j < end; j++) {
      x = gaussian_random(mean[i].x, std[i].x);
      y = gaussian_random(mean[i].y, std[i].y);
      //theta = normalize_theta_deg(gaussian_random(mean[i].theta, 
      //std[i].theta));
      theta = normalize_theta_rad(gaussian_random(mean[i].theta, 
						  std[i].theta));
      filter->particles[j].x = x;
      filter->particles[j].y = y;
      filter->particles[j].theta = theta;
      filter->particles[j].weight = 1.0/filter->num_particles;
    }
  }

  filter->initialized = 1;
  filter->sum_weight = 1;

  /*
  //  filter->first_odometry = 1;
  //  if (num_modes < 2)
  //    filter->global_mode = 0;
  //  else
  //    filter->global_mode = 1;
  //  filter->distance_travelled = 0;
  */

}


// 単数パーティクルの初期化

void localizer_initialize_particles_gaussian(LocalizerParticleFilter *filter, Point mean, Point std)
{
  localizer_initialize_particles_gaussians(filter, 1, &mean, &std);
}



inline
double gaussian(double mean,double std,double x)
{
  return (1/sqrt(2*M_PI*sqrt(std))) * exp(-SQUARE((x-mean))/(2*std));
}

/*
 尤度計算
 点マップ全ての点から距離誤差が最小になる候補を選出し，全候補の距離誤差の和から尤度を計算
*/

double likelihood_field_range_finder_model(LaserRangeData *laser, 
					   LocalizerParticle pose, 
					   PointMap *map)
{
  double q = 1;
  int i,j;
  double x,y,min_x,min_y;
  double theta;
  double min_dist = 0 ,dist = 0;
  //double zhit = 1.0 , zrand = 80.0, zmax=300.0;
  double zhit = 2.0 , zrand = 80.0, zmax=300.0;
  //for(i = 0 ; i < laser->total_points ; i+=10){

  //for( i = 0 ; i < laser->total_points; i+= (int)(abs(90-i)/4 + 10)){
  for( i = 0 ; i < laser->total_points; i+= 30){
    //if(laser->range[i] > 59) // 最大値は無視
      //continue;
    //fprintf(stderr, "%d ", i);
    //x = pose.x + laser->range[i]*cos((double)(i+pose.theta)/180.0*M_PI);
    //y = pose.y + laser->range[i]*sin((double)(i+pose.theta)/180.0*M_PI);
    theta = i * laser->resolution + laser->offset_fov + pose.theta;
    x = pose.x + laser->range[i] * cos(theta);
    y = pose.y + laser->range[i] * sin(theta);
    //min_dist = 8000*8000;
    min_dist = 80*80;
    
    for( j = 0 ; j < map->numdata ; j++){
      dist = SQUARE(x - map->data[j].x) + SQUARE(y - map->data[j].y); 
      if ( min_dist > dist ){
	min_dist = dist;
	min_x = map->data[j].x;
	min_y = map->data[j].y;
      }
    }
    // for debug
    /* 	  printf("{%0.3lf,%0.3lf}=>{%0.3lf,%0.3lf}, %0.3lf,%lf\n", */
    /* 		 x,y,min_x,min_y,sqrt(min_dist),gaussian(0,30,sqrt(min_dist))); */
    /* 	  set1[k].x = x, set1[k].y = y; */
    /* 	  set2[k].x = min_x, set2[k].y = min_y; */
    /* 	  k++; */

    q = q * (zhit * gaussian(0,1,sqrt(min_dist)) + zrand/zmax);
  }
  return q;
}

// 尤度計算
// 占有グリッドマップをピックアップテーブルとして利用し，グリッド内の値を0.0〜1.0の尤度として算出

double likelihood_field_range_finder_model(LaserRangeData *laser, 
					   LocalizerParticle pose, 
					   OccupancyGridMap *map)
{
  double q = 0;
  int i;
  int x, y, grid;
  double theta;

  x = pose.x/map->resolution - map->origin_x;
  y = pose.y/map->resolution - map->origin_y;
  grid = map->width*y+x;
  
  //if(grid<0 || grid>=map->width*map->height)
  if(x < 0 || x > map->width || y < 0 || y > map->height)
  {
    return 0;
  }else if(map->data[grid] < 200){
    return 0;
  }

  for( i = 0 ; i < laser->total_points; i++){
	double len_x, len_y;
	int n = 20;
	int is_valid = 1;

    theta = i * laser->resolution + laser->offset_fov + pose.theta;

	 /* 厳密には，正->切り捨て，負->切り下げ */

	len_x = laser->range[i] * cos(theta)/n;
	len_y = laser->range[i] * sin(theta)/n;

	// check n of points...

	for(int j=1, is_valid=1 ; j <= n && is_valid == 1; j++){
      x = (pose.x + len_x * j)/map->resolution - map->origin_x;
      y = (pose.y + len_y * j)/map->resolution - map->origin_y;
      grid = map->width*y+x;

      if(x < 0 || x > map->width || y < 0 || y > map->height || laser->range[i] >= 10 )
	  {
		is_valid = 0;
        break;
	  }else if((j < n && map->data[grid] < 50) || (j == n && map->data[grid] > 200) ){
        is_valid = 0;
		break;
	  }
	}

	if(is_valid == 1){
      q += 1;
	}
  }
  return q/laser->total_points;
}



int compare_particle(const void * a, const void * b)
{
	if(((LocalizerParticle *)a)->weight < ((LocalizerParticle *)b)->weight) {
		return 1;
	}else if(((LocalizerParticle *)a)->weight == ((LocalizerParticle *)b)->weight){
		return 0;
	}
	return -1;

/*
  return ((LocalizerParticle *)a)->weight < ((LocalizerParticle *)b)->weight;
  */
}


void localizer_particle_resample(LocalizerParticleFilter *filter)
{
  LocalizerParticle *new_particles;
  int i=0,j=0;
  double weight_sum=0;
  double dstep,step=0;

  new_particles = (LocalizerParticle *)
    calloc(filter->num_particles, sizeof(LocalizerParticle));

  qsort(filter->particles, filter->num_particles,
	sizeof(LocalizerParticle), compare_particle);

#ifdef RESAMPLE_DEBUG

  for( i = 0 ; i < filter->num_particles; i++){
    printf("befor %lf  (%lf %lf %lf)\n",
	   filter->particles[i].weight,
	   filter->particles[i].x,
	   filter->particles[i].y,
	   filter->particles[i].theta
	   );
  }

#endif

  dstep =  filter->sum_weight/ filter->num_particles;
  weight_sum = filter->particles[0].weight;;
  j = 0;

  // particleの重みが高い物を取り出す

  for( i = 0 ; i < filter->num_particles; i++){
    if (weight_sum < dstep*i){
      j++;
      weight_sum+=filter->particles[j].weight;
    }

    new_particles[i] = filter->particles[j];
    step +=dstep;

    //printf("%lf %d\n", weight_sum,j);

  }

#ifdef RESAMPLE_DEBUG

  for( i = 0 ; i < filter->num_particles;i++){
    printf("middle %lf  (%.2lf %.2lf %.2lf)\n", 
	   new_particles[i].weight, 
	   new_particles[i].x, 
	   new_particles[i].y,
	   new_particles[i].theta
	   );

  }
     
#endif

  for( i = 0 ; i < filter->num_particles ; i++){
      
    new_particles[i].x  = gaussian_random( new_particles[i].x , 0.1);
    new_particles[i].y = gaussian_random( new_particles[i].y, 0.1);
    //new_particles[i].theta = normalize_theta_deg(gaussian_random(new_particles[i].theta,20));
    new_particles[i].theta = normalize_theta_rad(gaussian_random(new_particles[i].theta,DEG2RAD(20)));
    new_particles[i].weight = 0.0;
  }
     
  free(filter->particles);
  filter->particles = new_particles;

}


// オドメトリ動作モデルによるサンプリング

void sample_motion_model_odometry(LocalizerParticleFilter *filter, LocalizerOdometry *odometry, LocalizerOdometry *prev_odometry)
{

  LocalizerParticle *new_particles;
  int i=0,j=0;
  double weight_sum=0;
  double dstep,step=0;

  new_particles = (LocalizerParticle *)
    calloc(filter->num_particles, sizeof(LocalizerParticle));

  qsort(filter->particles, filter->num_particles,
	sizeof(LocalizerParticle), compare_particle);

#ifdef RESAMPLE_DEBUG
  for( i = 0 ; i < filter->num_particles; i++){
    printf("before %lf  (%lf %lf %lf)\n",
	   filter->particles[i].weight,
	   filter->particles[i].x,
	   filter->particles[i].y,
	   filter->particles[i].theta
	   );
  }
#endif

  dstep =  filter->sum_weight/ filter->num_particles;
  weight_sum = filter->particles[0].weight;;
  j = 0;

  // particleの重みが高い物を取り出す

  for( i = 0 ; i < filter->num_particles; i++){
/*
    if (weight_sum < dstep*i){
      j++;
      weight_sum+=filter->particles[j].weight;
    }
	
    new_particles[i] = filter->particles[j];
*/
	new_particles[i].weight  = 0;
	new_particles[i].x =   odometry->x;
	new_particles[i].y =   odometry->y;
	new_particles[i].theta =  odometry->theta;

    step +=dstep;

    //printf("%lf %d\n", weight_sum,j);

  }

#ifdef RESAMPLE_DEBUG

  for( i = 0 ; i < filter->num_particles;i++){
    printf("middle %lf  (%.2lf %.2lf %.2lf)\n", new_particles[i].weight, new_particles[i].x, new_particles[i].y,new_particles[i].theta );
  }
     
#endif

  // オドメトリの誤差パラメータ


  const double alpha1 = ODOMETRY_ERROR_ALPHA1;
  const double alpha2 = ODOMETRY_ERROR_ALPHA2;
  const double alpha3 = ODOMETRY_ERROR_ALPHA3;
  const double alpha4 = ODOMETRY_ERROR_ALPHA4;

  // オドメトリの相対量
  
  double dx, dy, dtheta;
  
  // オドメトリからの相対動作パラメータ
  
  double drot1, dtrans, drot2;
  
  // オドメトリと誤差パラメータから推測される相対動作パラメータ
  
  double drot1_hat, dtrans_hat, drot2_hat;
  
  for( i = 0 ; i < filter->num_particles ; i++){
    dx = (odometry->x - prev_odometry->x) ;
    dy = (odometry->y - prev_odometry->y) ;
    dtheta = (odometry->theta - prev_odometry->theta) ;

    drot1  = atan2(dy, dx) - prev_odometry->theta;
    dtrans = sqrt(dx*dx + dy*dy);
    drot2  = dtheta - drot1;

    drot1_hat  = drot1  - gaussian_random(0, sqrt(alpha1 * drot1 * drot1 + alpha2 * dtrans * dtrans)) ;
    dtrans_hat = dtrans - gaussian_random(0, sqrt(alpha3 * dtrans * dtrans + alpha4 * drot1 * drot1 + alpha4 * drot2 * drot2)) ;
    drot2_hat  = drot2  - gaussian_random(0, sqrt(alpha1 * drot2 * drot2 + alpha2 * dtrans * dtrans));
    
    new_particles[i].x = new_particles[i].x + dtrans_hat * cos(new_particles[i].theta + drot1_hat);
    new_particles[i].y = new_particles[i].y + dtrans_hat * sin(new_particles[i].theta + drot1_hat);
    new_particles[i].theta = normalize_theta_rad( new_particles[i].theta + drot1_hat + drot2_hat );
    new_particles[i].weight = 0.0;
  }
  
  free(filter->particles);
  filter->particles = new_particles;

  prev_odometry->x = odometry->x;
  prev_odometry->y = odometry->y;
  prev_odometry->theta = odometry->theta;
}
